{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "c5190c1d",
   "metadata": {},
   "source": [
    "------ standard imports ------ #"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "11c55d3e",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 15: Vision-Based Control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d202971",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy import linalg\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(linewidth=120, formatter={'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0d46c5c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.random.seed(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "404eb9c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "from machinevisiontoolbox.base import *\n",
    "from machinevisiontoolbox import *\n",
    "from spatialmath.base import *\n",
    "from spatialmath import *"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b3405b2",
   "metadata": {},
   "source": [
    "------------------------------ #"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "88c82619",
   "metadata": {},
   "source": [
    "# 15.1 Position-Based Visual Servoing\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "49743399",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default(pose=SE3.Trans(1, 1, -2));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02709d05",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = mkgrid(2, 0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "17d263d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = camera.project_point(P, objpose=SE3.Tz(1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f3e511a",
   "metadata": {},
   "outputs": [],
   "source": [
    "Te_C_G = camera.estpose(P, p, frame=\"camera\");\n",
    "Te_C_G.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ae91cce",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_Cd_G = SE3.Tz(1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ef3808f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_delta = Te_C_G * T_Cd_G.inv();\n",
    "T_delta.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "783c96ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.pose = camera.pose * T_delta.interp1(0.05);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca15e914",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default(pose = SE3.Trans(1, 1, -2));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5e45a1b1",
   "metadata": {},
   "outputs": [],
   "source": [
    "T_Cd_G = SE3.Tz(1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d84b0f9",
   "metadata": {},
   "outputs": [],
   "source": [
    "pbvs = PBVS(camera, P=P, pose_g=SE3.Trans(-1, -1, 2), pose_d=T_Cd_G, plotvol=[-1, 2, -1, 2, -3, 2.5])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "926ba66c",
   "metadata": {},
   "outputs": [],
   "source": [
    "pbvs.run(200);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1b84dc83",
   "metadata": {},
   "outputs": [],
   "source": [
    "pbvs.plot_p();     # plot image plane trajectory\n",
    "pbvs.plot_vel();   # plot camera velocity\n",
    "pbvs.plot_pose();  # plot camera trajectory"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bbf815bb",
   "metadata": {},
   "source": [
    "# 15.2 Image-Based Visual Servoing\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9312d857",
   "metadata": {},
   "source": [
    "## 15.2.1 Camera and Image Motion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15c14b5a",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c1c8f84f",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = [1, 1, 5];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4ed2b5e4",
   "metadata": {},
   "outputs": [],
   "source": [
    "p0 = camera.project_point(P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f51f8a10",
   "metadata": {},
   "outputs": [],
   "source": [
    "p_dx = camera.project_point(P, pose=SE3.Tx(0.1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b231c931",
   "metadata": {},
   "outputs": [],
   "source": [
    "(p_dx - p0) / 0.1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3fe62c0e",
   "metadata": {},
   "outputs": [],
   "source": [
    "(camera.project_point(P, pose=SE3.Tz(0.1) ) - p0) / 0.1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cf96570e",
   "metadata": {},
   "outputs": [],
   "source": [
    "(camera.project_point(P, pose=SE3.Rx(0.1)) - p0) / 0.1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d77b261",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = camera.visjac_p(p0, depth=5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6bd62852",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.flowfield([1, 0, 0, 0, 0, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eb6301c8",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.flowfield([0, 0, 1, 0, 0, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2fa6ce96",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.flowfield([0, 0, 0, 0, 0, 1]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cd8f7078",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.flowfield([0, 0, 0, 0, 1, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ccd6eb3",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.visjac_p(camera.pp, depth=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b59d7645",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.f = 20e-3;\n",
    "camera.flowfield([0, 0, 0, 0, 1, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cd3e41ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.f = 4e-3;\n",
    "camera.flowfield([0, 0, 0, 0, 1, 0]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b2076e96",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = camera.visjac_p(camera.pp, depth=1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5495fb75",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.null_space(J)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a1acc232",
   "metadata": {},
   "source": [
    "## 15.2.2 Controlling Feature Motion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81b47b4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default(pose=SE3.Trans(1, 1, -2));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4483a3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = mkgrid(2, side=0.5, pose=SE3.Tz(3));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "56f2a7f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "pd = 200 * np.array([[-1, -1, 1, 1], [-1, 1, 1, -1]]) + np.c_[camera.pp]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "647d6b02",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = camera.project_point(P)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "33e8236f",
   "metadata": {},
   "outputs": [],
   "source": [
    "e = pd - p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "623a531b",
   "metadata": {},
   "outputs": [],
   "source": [
    "J = camera.visjac_p(p, depth=1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95dbbbf8",
   "metadata": {},
   "outputs": [],
   "source": [
    "lmbda = 0.1;\n",
    "v = lmbda * np.linalg.pinv(J) @ e.flatten(order=\"F\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1ecb5e72",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.pose = camera.pose @ SE3.Delta(v);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b6f9d241",
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = CentralCamera.Default(pose=SE3.Trans(1, 1, -3) * SE3.Rz(0.6));\n",
    "ibvs = IBVS(camera, P=P, p_d=pd);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "59a3fdcd",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.run(25);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a89df803",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.plot_p();     # plot image plane trajectory\n",
    "ibvs.plot_vel();   # plot camera velocity\n",
    "ibvs.plot_pose();  # plot camera trajectory "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d586be0",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.plot_jcond();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b8b05b3",
   "metadata": {},
   "outputs": [],
   "source": [
    "%run -m IBVS-main -H"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1a10ed9b",
   "metadata": {},
   "outputs": [],
   "source": [
    "out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b16510cb",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(out.t, out.y2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cb104d37",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(out.clock0.t, out.clock0.x)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3792159b",
   "metadata": {},
   "source": [
    "## 15.2.3 Estimating Feature Depth\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc2bf76d",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs = IBVS(camera, P=P, p_d=pd, depth=1);\n",
    "ibvs.run(50)\n",
    "ibvs = IBVS(camera, P=P, p_d=pd, depth=10);\n",
    "ibvs.run(50)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "88d84407",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs = IBVS(camera, P=P, p_d=pd, depthest=True);\n",
    "ibvs.run()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "29ccf7f0",
   "metadata": {},
   "source": [
    "## 15.2.4 Performance Issues\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6ad26b14",
   "metadata": {},
   "outputs": [],
   "source": [
    "pbvs.pose_0 = SE3.Trans(-2.1, 0, -3) * SE3.Rz(5*pi/4);\n",
    "pbvs.run()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ab2ddbb",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.pose_0 = pbvs.pose_0;\n",
    "ibvs.run()\n",
    "ibvs.plot_p();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3d1e98c",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.pose_0 = SE3.Tz(-1) * SE3.Rz(2);\n",
    "ibvs.run(50)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e73f0e9f",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs.pose_0 = SE3.Tz(-1) * SE3.Rz(pi);\n",
    "ibvs.run(10)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "389168d0",
   "metadata": {},
   "source": [
    "# 15.3 Using Other Image Features\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b65a9aa9",
   "metadata": {},
   "source": [
    "## 15.3.1 Line Features\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27859112",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = circle([0, 0, 3], 0.5, resolution=3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4931e8ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs = IBVS_l.Example(camera);  # quick problem setup\n",
    "ibvs.run()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4838e33e",
   "metadata": {},
   "source": [
    "## 15.3.2 Ellipse Features\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5aefc7e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = circle([0, 0, 3], 0.5, resolution=10);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "55d09b83",
   "metadata": {},
   "outputs": [],
   "source": [
    "p = camera.project_point(P, pose=camera.pose, retinal=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f4f623d",
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y = p\n",
    "A = np.column_stack([y**2, -2*x*y, 2*x, 2*y, np.ones(x.shape)]);\n",
    "b = -(x**2);\n",
    "E, *_ = np.linalg.lstsq(A, b, rcond=None)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c288f8c",
   "metadata": {},
   "outputs": [],
   "source": [
    "plane = [0, 0, 1, -3];  # plane Z=3\n",
    "J = camera.visjac_e(E, plane);\n",
    "J.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "56d50e1d",
   "metadata": {},
   "outputs": [],
   "source": [
    "ibvs = IBVS_e.Example();  # quick problem setup\n",
    "ibvs.run()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0f9ceb9e",
   "metadata": {},
   "source": [
    "## 15.3.3 Photometric Features\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "82fdd4e9",
   "metadata": {},
   "source": [
    "# 15.4 Wrapping Up\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "294ca8f5",
   "metadata": {},
   "source": [
    "## 15.4.1 Further Reading\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "243f4c28",
   "metadata": {
    "lines_to_next_cell": 2
   },
   "source": [
    "## 15.4.2 Exercises\n"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python",
   "version": "3.8.5 (default, Sep  4 2020, 02:22:02) \n[Clang 10.0.0 ]"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
